#include "kinematic.hpp"
/*****文件说明*********
机械臂的正逆运动学
除非特别说明，以下所提到的角度均为DH参数，单位rad；
坐标均为世界坐标系下的坐标；
关节i位于坐标系i原点
**********************/

//11.13测量数据
//38.9/2164=0.000313739870159794
//64.3/3630=0.0003091588730116677
//73.8/3144=0.00040968606487653154
//90/1860=0.0008445141541908046

RobotArm::RobotArm()
{
    L0 = 170;
    L1 = 64.2;
    L2 = 305;
    L4 = 222.63;
    L5 = 36.25;
    R[0] = zhnmat::Mat(3, 3);
    R[1] = zhnmat::Mat(3, 3);
    R[2] = zhnmat::Mat(3, 3);
    R[3] = zhnmat::Mat(3, 3);
    R[4] = zhnmat::Mat(3, 3);
    R[5] = zhnmat::Mat(3, 3);
    P[0] = zhnmat::Vector3d(0, 0, L0);
    P[1] = zhnmat::Vector3d(L1,0, 0);
    P[2] = zhnmat::Vector3d(L2,0, 0);
    P[3] = zhnmat::Vector3d(0, L4, 0);
    P[4] = zhnmat::Vector3d(0, 0, 0);
    P[5] = zhnmat::Vector3d(0, L5, 0);
    PulseCenter = std::array<int, 6> {0, 2164, 3630, 3144, 1800, 0};
    PulseWorking = std::array<int, 6> {3079,2604,2755,1482,2426,2733};
    PulseSleep = std::array<int, 6> {300, 2100, 1100, 800, 1800, 0};
    NegRadLim = std::array<double, 6> {3.08, -2.2525, 1.1206, 1.2847512101, 1.442, 0};
    DriveRatio[0] = -0.00039040259584086252;
    DriveRatio[1] = +0.00031559378255242008;
    DriveRatio[2] = -0.00031126891118372262;
    DriveRatio[3] = -0.0004086358810600667;
    DriveRatio[4] = -0.00080113473862040074;
    DriveRatio[5] = -0.000408341183485465;
    _Vmove = 25;
    _Eff = zhnmat::Vector3d(-30, -80, 4);
    PulseCalibrate.push_back(std::array<int, 6>{2700,3600,2500,-1700,3500,0});
    PulseCalibrate.push_back(std::array<int, 6>{2500,3600,2500,-1700,4200,0});
    PulseCalibrate.push_back(std::array<int, 6>{2000,4500,2800,-1700,4200,0});
    PulseCalibrate.push_back(std::array<int, 6>{2000,4200,3700,-1700,4200,0});
    PulseCalibrate.push_back(std::array<int, 6>{2600,4200,3700,-1700,4200,0});
    PulseCalibrate.push_back(std::array<int, 6>{2600,4900,3700,-1700,4200,0});
    PulseCalibrate.push_back(std::array<int, 6>{2600,4700,3100,-1900,3600,0});
    PulseUser.push_back(std::array<int, 6>{2316,4748,2485,3144,1493,1660});
    PulseUser.push_back(std::array<int, 6>{2595,4637,1991,3820,1530,1690});
    PulseUser.push_back(std::array<int, 6>{2501,4985,3245,3550,1185,1920});
    PulseUser.push_back(std::array<int, 6>{2787,4828,2748,3690,1328,2100});
    PulseUser.push_back(std::array<int, 6>{2261,5194,3850,3448,1023,1750});
    PulseUser.push_back(std::array<int, 6>{2037,4960,3168,3441,1185,1570});
    PulseUser.push_back(std::array<int, 6>{1773,4782,2590,3417,1329,1300});
}

/**********************
正运动学求解末端坐标
* angle:输入,各关节角度
* posture:输出,末端坐标和姿态
**********************/
void RobotArm::Forward_Solve(const double angle[], double posture[], FORWARD_MODE mode)
{
    using namespace std;
    using namespace zhnmat;
    R[0] = Mat(3, 3, vector<double>{cos(angle[0]),-sin(angle[0]),0,sin(angle[0]),cos(angle[0]),0,0,0,1});
    R[1] = Mat(3, 3, vector<double>{cos(angle[1]),-sin(angle[1]),0,0,0,1,-sin(angle[1]),-cos(angle[1]),0});
    R[2] = Mat(3, 3, vector<double>{cos(angle[2]),-sin(angle[2]),0,sin(angle[2]),cos(angle[2]),0,0,0,1});
    R[3] = Mat(3, 3, vector<double>{cos(angle[3]),-sin(angle[3]),0,0,0,1,-sin(angle[3]),-cos(angle[3]),0});
    R[4] = Mat(3, 3, vector<double>{cos(angle[4]),-sin(angle[4]),0,0,0,-1,sin(angle[4]),cos(angle[4]),0});
    R[5] = Mat(3, 3, vector<double>{cos(angle[5]),-sin(angle[5]),0,0,0,1,-sin(angle[5]),-cos(angle[5]),0});
    Vector3d origin5, origin6;  // 关节5,关节6坐标
    for (int i=4; i>=0; --i)
        origin5 = R[i]*origin5 + P[i];
    if (mode==JOINT5){
        posture[0]=origin5._x; posture[1]=origin5._y; posture[2]=origin5._z;
        return;
    }
    for (int i=5; i>=0; --i)
        origin6 = R[i]*origin6 + P[i];
    if (mode==JOINT6){
        posture[0]=origin6._x; posture[1]=origin6._y; posture[2]=origin6._z;
        return;
    }
    Vector3d eff = _Eff;
    for (int i=5; i>=0; --i)
        eff = R[i]*eff + P[i];
    posture[0] = eff._x; posture[1] = eff._y; posture[2] = eff._z;
    if (mode==EFFECTOR) return;
    Vector3d vecL5 = origin6 - origin5;  // 连杆5向量，从关节5指向关节6
    posture[5] = atan2(-vecL5._x, vecL5._y);  // 方位角
    posture[4] = atan2(vecL5._z, sqrt(vecL5._x*vecL5._x + vecL5._y*vecL5._y));  // 俯仰角
    eff = Vector3d(0, -1, 0);
    for (int i=5; i>=0; --i)
        eff = R[i]*eff + P[i];
    Vector3d vecL6 = eff - origin6;  // 连杆6向量，从关节6指向末端点
    Vector3d vec5M = Vec_vertical(vecL5);  // 连杆5向量的平行向量,即横滚角基准向量
    double costheta = Vec_angle(vecL6, vec5M);  // 两个向量之间的夹角绝对值
    posture[3] = acos(costheta);  // 夹角为横滚角的绝对值
    Vector3d cross = vecL6 & vec5M;  // 法向量叉积判断横滚角正负
    if (Vec_angle(cross, vecL5) < 0)  // 如果法向量的叉积向量等于L5向量则横滚角为正，否则为负
        posture[3] = -posture[3];
}

/**********************
逆运动学求解各关节角度
* posture:输入,目标点坐标和姿态
* angle:输出,各关节角度
* return:逆解是否存在.0,存在;其它,不存在
**********************/
int RobotArm::Inverse_Solve(const double posture[], double angle[])
{
    using namespace std;
    using namespace zhnmat;

    /*计算各关节坐标*/
    double x = -cos(posture[4])*sin(posture[5]);
    double y = cos(posture[4])*cos(posture[5]);
    double z = sin(posture[4]);
    Vector3d vecL5(x, y, z);  // 连杆5单位向量,从关节5指向关节6
    vecL5 *= (L5+_Eff._z);  // 连杆5向量
    Vector3d vec5M = Vec_vertical(vecL5).Normalize();  // 横滚角基准单位向量
    Vector3d vec5N = (vec5M & vecL5).Normalize();  //用于将基准向量进行旋转
    Vector3d vecL6 = vec5M*cos(posture[3]) + vec5N*sin(posture[3]);  // 横滚角基准向量按横滚角旋转后的向量，即连杆6向量
    vecL6 = (vecL6.norm2()==0) ? Vector3d(_Eff._y, 0, 0) : vecL6*(-_Eff._y);  // 若连杆5垂直于地面形成万向节死锁,则连杆6向量自定义
    Vector3d originEff(posture[0], posture[1], posture[2]);  // 末端点的世界坐标
    Vector3d origin6 = originEff - vecL6;  // 关节6的yz伪坐标
    Vector3d origin5 = origin6 - vecL5;  // 关节5伪坐标
    Vector3d vecdelta = (vecL5 & vecL6);  // 真伪坐标切换向量
    vecdelta = -_Eff._x * vecdelta.Normalize();
    origin5 += vecdelta;  // 关节5实际坐标
    origin6 += vecdelta;  // 关节6的z伪坐标

    /*计算关节1,2,3角度*/
    x = origin5._x; y = origin5._y; z = origin5._z - L0;
    double r = x * x + y * y + z * z;  // 关节5至关节1的距离
    double costheta, pitch;
    if (r > (L1 + L2 + L4) * (L1 + L2 + L4)) return 1;  // 目标点超出工作距离
    angle[0] = atan2(y, x);  // 关节1角度
    r = sqrt(x * x + y * y) - L1;  // 关节5投影至关节1(2)所在XOY平面与关节2距离
    pitch = atan2(z, r);  // 关节5相对于关节2仰角
    r = sqrt(r * r + z * z);  // 关节5与关节2直线距离
    costheta = (L2 * L2 + r * r - L4 * L4) / L2 / r / 2.0;  // 关节5与关节2连线与L2夹角
    if (costheta > 1 || costheta < -1) return 2;  // 关节2角度无法计算
    pitch += acos(costheta);  // L2仰角
    angle[1] = -pitch;  // 关节2角度
    costheta = (L4 * L4 + L2 * L2 - r * r) / L2 / L4 / 2.0;  // L2与L4夹角
    if (costheta > 1 || costheta < -1) return 3;  // 关节3角度无法计算
    angle[2] = M_PI_2 - acos(costheta);  // 关节3角度
    x = (L2 * cos(pitch) + L1) * cos(angle[0]);
    y = (L2 * cos(pitch) + L1) * sin(angle[0]);
    z = L2 * sin(pitch) + L0;
    Vector3d origin3(x, y, z);  // 关节3坐标

    /*计算关节4,5角度*/
    // 若连杆4垂直于地面形成万向节死锁,则关节4角度由连杆5和关节1共同决定,此处未考虑
    // 关节4角度被限制在±90°之间
    Vector3d vecL4 = origin5 - origin3;  // 连杆4向量,从关节3指向关节5(坐标系4与5的原点重合)
    Vector3d vec4M = Vec_vertical(vecL4);  // 连杆4向量的平行向量
    Vector3d vec4N = vecL5 & vecL4;  // 该向量为关节5的旋转轴,垂直于连杆4和连杆5
    costheta = Vec_angle(vec4M, vec4N);
    bool reverse = false;
    if (costheta<0){  // 限制关节4角度
        costheta = -costheta;
        vec4M.Reverse();
        reverse = true;
    }
    angle[3] = acos(costheta);  // 关节4角度
    Vector3d cross = vec4M & vec4N;  // 法向量叉积判断正负
    if (Vec_angle(cross, vecL4) < 0)  // 如果法向量的叉积向量等于L4向量则为正，否则为负
        angle[3] = -angle[3];
    if (vec4N.norm2()==0)  // 连杆4和连杆5在同一条直线上
        angle[3] = 0;
    costheta = Vec_angle(vecL4, vecL5);
    angle[4] = acos(costheta);  // 关节5角度
    if (!reverse)
        angle[4] = -angle[4];

    /*计算关节6角度*/
    angle[5] = 0;  // 设关节6角度为0
    vecL6 = originEff;
    originEff = _Eff;
    R[0] = Mat(3, 3, vector<double>{cos(angle[0]),-sin(angle[0]),0,sin(angle[0]),cos(angle[0]),0,0,0,1});
    R[1] = Mat(3, 3, vector<double>{cos(angle[1]),-sin(angle[1]),0,0,0,1,-sin(angle[1]),-cos(angle[1]),0});
    R[2] = Mat(3, 3, vector<double>{cos(angle[2]),-sin(angle[2]),0,sin(angle[2]),cos(angle[2]),0,0,0,1});
    R[3] = Mat(3, 3, vector<double>{cos(angle[3]),-sin(angle[3]),0,0,0,1,-sin(angle[3]),-cos(angle[3]),0});
    R[4] = Mat(3, 3, vector<double>{cos(angle[4]),-sin(angle[4]),0,0,0,-1,sin(angle[4]),cos(angle[4]),0});
    R[5] = Mat(3, 3, vector<double>{cos(angle[5]),-sin(angle[5]),0,0,0,1,-sin(angle[5]),-cos(angle[5]),0});
    for (int i=5; i>=0; --i)
        originEff = R[i]*originEff + P[i];
    originEff -= origin6;  // 关节6角度为0时的末端向量，从关节6的z伪坐标指向关节6角度为0时的末端点
    vecL6 -= origin6;  // 实际末端向量，从关节6的z伪坐标指向当前实际末端点
    costheta = Vec_angle(originEff, vecL6);
    angle[5] = acos(costheta);  // 关节6角度
    cross = originEff & vecL6;  // 向量叉积用于判断正负
    if (Vec_angle(cross, vecL5) < 0)  // 如果法向量的叉积向量等于L5向量则为正，否则为负
        angle[5] = -angle[5];
    return 0;
}

KINEMATIC_EXPORT PythonJoint6 Python_Joint_Forward(PythonJoint6 a)
{
    RobotArm ar3;
    double angle[6]{ a.j0,a.j1,a.j2,a.j3,a.j4,a.j5 };
    double posture[6];
    ar3.Forward_Solve(angle, posture);
    PythonJoint6 ans;
    ans.j0 = posture[0];
    ans.j1 = posture[1];
    ans.j2 = posture[2];
    ans.j3 = posture[3];
    ans.j4 = posture[4];
    ans.j5 = posture[5];
    return ans;
}
KINEMATIC_EXPORT PythonJoint6 Python_Joint_Inverse(PythonJoint6 a)
{
    RobotArm ar3;
    double posture[6]{ a.j0,a.j1,a.j2,a.j3,a.j4,a.j5 };
    double angle[6];
    ar3.Inverse_Solve(posture, angle);
    PythonJoint6 ans;
    ans.j0 = angle[0];
    ans.j1 = angle[1];
    ans.j2 = angle[2];
    ans.j3 = angle[3];
    ans.j4 = angle[4];
    ans.j5 = angle[5];
    return ans;
}
